#include "ArmMover.h"

#define PI 3.14159265
#define ZERO_DIV_CORRECTION 0.00001526 // 1*2^-16 = shift left 16
#define NEG_ROTATE_CORRECTION 0.00001526 // 1*2^-16 = shift left 16

// length (cm) of each part of the arm
#define UPPER_ARM 35.0
#define LOWER_AND_MIDDLE_ARM 50.0
#define MIDDLE_ARM 25.0
#define MIDDLE_AND_UPPER_ARM 58.0
#define LOWER_ARM 27.0 

#define MAX_STEPS 60 // maximal steps number of the fingers towards center axis (closed position)

using namespace std;

ArmMover::ArmMover() :m_loop_rate(10000) {

	//Setting up the publishers
	m_arm_upper_elbow_pub = m_nodeHandler.advertise<std_msgs::Float64>("/elbow2_controller/command", 1000);
	m_arm_lower_elbow_pub = m_nodeHandler.advertise<std_msgs::Float64>("/elbow1_controller/command", 1000);
	m_arm_shoulder_pub = m_nodeHandler.advertise<std_msgs::Float64>("/shoulder_controller/command", 1000);
	m_arm_wrist_pub = m_nodeHandler.advertise<std_msgs::Float64>("/wrist_controller/command", 1000);
	m_arm_base_rotation_pub = m_nodeHandler.advertise<std_msgs::Float64>("/base_rotation_controller/command", 1000);
	m_arm_left_finger_pub = m_nodeHandler.advertise<std_msgs::Float64>("/left_finger_controller/command", 1000);
	m_arm_right_finger_pub = m_nodeHandler.advertise<std_msgs::Float64>("/right_finger_controller/command", 1000);

	//Setting up the subscribers to topics that inform us current arm states (joint load, position etc.)
	m_arm_right_finger_state_sub = m_nodeHandler.subscribe<dynamixel_msgs::JointState>("/right_finger_controller/state", 10,
			&ArmMover::rightFingerStateCb, this);
	m_arm_left_finger_state_sub = m_nodeHandler.subscribe<dynamixel_msgs::JointState>("/left_finger_controller/state", 10,
			&ArmMover::leftFingerStateCb, this);

	usleep(1500000);

	setStartPosition();
}

void ArmMover::setStartPosition() {

	MoveShoulder(0.3);

	usleep(500000);

	MoveUpperElbow(-1.8);
	MoveLowerElbow(1.6);
	MoveWrist(0.0);
	RotateBase(0.0);

	MoveLeftFinger(1.5); //opened
	MoveRightFinger(-1.5); //opened

	usleep(500000);
	MoveShoulder(1.6);
}

void ArmMover::liftObject() {

	MoveShoulder(0.3);

	usleep(500000);

	MoveUpperElbow(-1.8);
	MoveLowerElbow(1.6);
	MoveWrist(0.0);
	RotateBase(0.0);

	usleep(500000);
	MoveShoulder(1.6);
}

void ArmMover::rightFingerStateCb(const dynamixel_msgs::JointState::ConstPtr& msg) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	m_right_finger_load = msg->load;
	m_right_finger_cur_pos = msg->current_pos;
	mlock.unlock();
}

void ArmMover::leftFingerStateCb(const dynamixel_msgs::JointState::ConstPtr& msg) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	m_left_finger_load = msg->load;
	m_left_finger_cur_pos = msg->current_pos;
	mlock.unlock();
}

bool ArmMover::squeezeObject() {

	float curr_right_finger_load;
	float curr_left_finger_load;

	openFingers();
	sleep(2);

	int num_steps = 0;

	// do while fingers are not close enough to each other
	while (num_steps < MAX_STEPS) {
		++num_steps;

		ros::spinOnce();

		// update the loads at fingers
		std::unique_lock<std::mutex> mlock(m_mutex);
		curr_right_finger_load = m_right_finger_load;
		curr_left_finger_load = m_left_finger_load;
		mlock.unlock();

		// check if current load at both fingers is significant to claim that
		// object is held
		if (curr_right_finger_load >= 0.2 || curr_left_finger_load >= 0.2) {
			ROS_WARN("Max load exceeded. curr_right_finger_load = %f, curr_left_finger_load = %f",
					curr_right_finger_load, curr_left_finger_load);
			break;
		}

		// move both fingers to center (close them)
		closeRightFinger(0.12);
		closeLeftFinger(0.1);
		usleep(35000);
	}

	// check if object is held by fingers
	// (if max_iterations bigger than 60, then load at both fingers
	//  not exceeded, that is object is not squeezed)
	if (num_steps >= MAX_STEPS) {
		return false;
	}

	// the object is held by fingers
	return true;
}

void ArmMover::MoveUpperElbow(double value) {

	// prepare message for publishing
	m_arm_upper_elbow_msg.data = value;

	// publish the message to motor
	m_arm_upper_elbow_pub.publish(m_arm_upper_elbow_msg);
	m_loop_rate.sleep();
	m_arm_upper_elbow_pub.publish(m_arm_upper_elbow_msg); // published twice to ensure response
}

void ArmMover::MoveLowerElbow(double value) {

	// prepare message for publishing
	m_arm_lower_elbow_msg.data = value;

	// publish the message to motor
	m_arm_lower_elbow_pub.publish(m_arm_lower_elbow_msg);
	m_loop_rate.sleep();
	m_arm_lower_elbow_pub.publish(m_arm_lower_elbow_msg); // published twice to ensure response
}

void ArmMover::MoveShoulder(double value) {

	// prepare message for publishing
	m_arm_shoulder_msg.data = value;

	// publish the message to motor
	m_arm_shoulder_pub.publish(m_arm_shoulder_msg);
	m_loop_rate.sleep();
	m_arm_shoulder_pub.publish(m_arm_shoulder_msg); // published twice to ensure response
}

void ArmMover::MoveWrist(double value) {

	// prepare message for publishing
	m_arm_wrist_msg.data = value;

	// publish the message to motor
	m_arm_wrist_pub.publish(m_arm_wrist_msg);
	m_loop_rate.sleep();
	m_arm_wrist_pub.publish(m_arm_wrist_msg); // published twice to ensure response
}

void ArmMover::RotateBase(double value) {

	// prepare message for publishing
	m_arm_base_rotation_msg.data = value;

	// publish the message to motor
	m_arm_base_rotation_pub.publish(m_arm_base_rotation_msg);
	m_loop_rate.sleep();
	m_arm_base_rotation_pub.publish(m_arm_base_rotation_msg); // published twice to ensure response
}

void ArmMover::MoveLeftFinger(double value) {

	// prepare message for publishing
	m_arm_left_finger_msg.data = value;

	// publish the message to motor
	m_arm_left_finger_pub.publish(m_arm_left_finger_msg);
	m_loop_rate.sleep();
	m_arm_left_finger_pub.publish(m_arm_left_finger_msg); // published twice to ensure response
}

void ArmMover::MoveRightFinger(double value) {

	// prepare message for publishing
	m_arm_right_finger_msg.data = value;

	// publish the message to motor
	m_arm_right_finger_pub.publish(m_arm_right_finger_msg);
	m_loop_rate.sleep();
	m_arm_right_finger_pub.publish(m_arm_right_finger_msg); // published twice to ensure response
}

// Open or close fingers throughly
void ArmMover::openRightFinger() {

	MoveRightFinger(-2);
}

void ArmMover::openLeftFinger() {

	MoveLeftFinger(2);
}

void ArmMover::closeRightFinger() {

	MoveRightFinger(0.6);
}

void ArmMover::closeLeftFinger() {

	MoveLeftFinger(-0.5);
}

void ArmMover::openFingers() {

	openRightFinger();
	openLeftFinger();
}

// Open or close fingers by step (For example: 0.1 or 0.5)
void ArmMover::openRightFinger(float step) {

	std::unique_lock<std::mutex> mlock(m_mutex);

	MoveRightFinger(m_right_finger_cur_pos - step);

	mlock.unlock();
}

void ArmMover::openLeftFinger(float step) {

	std::unique_lock<std::mutex> mlock(m_mutex);

	MoveLeftFinger(m_left_finger_cur_pos + step);

	mlock.unlock();
}

void ArmMover::closeRightFinger(float step) {

	std::unique_lock<std::mutex> mlock(m_mutex);

	MoveRightFinger(m_right_finger_cur_pos + step);

	mlock.unlock();
}

void ArmMover::closeLeftFinger(float step) {

	std::unique_lock<std::mutex> mlock(m_mutex);

	MoveLeftFinger(m_left_finger_cur_pos - step);

	mlock.unlock();
}

bool ArmMover::moveArm(double front, double horizontal, double vertical) {

	// Move arm according to vertical axis
	bool check_nans = moveVertical(getDistance(front, horizontal), vertical);

	// if (x,y) is out of arm's possible range, the check_nans will false,
	// then moveArm will return false
	if (!check_nans) {
		return false;
	}

	MoveLowerElbow(0.0);

	// if arm successfully moved at vertical axis, move also
	// according to horizontal
	moveHorizontal(front, horizontal);

	// the arm was moved successfully to (x,y,z) point
	return true;
}

bool ArmMover::moveVertical(double front, double vertical) {

	double alpha = 0;
	double gamma = 0;

	// calculate vertical axis triangle angles according to (front,vertical) point
	calcAngle(front, vertical, UPPER_ARM, LOWER_AND_MIDDLE_ARM, &alpha, &gamma);

	// convert the angles to motor positions
	float shoulder_motor_pos = getShoulderMotorPosition(alpha);
	float elbow_motor_pos = getElbowMotorPosition(gamma);

	// check given point in arm's range according to calculated angles (NaN's if out of range)
	if (isnan(alpha) || isnan(gamma) || isnan(shoulder_motor_pos) || isnan(elbow_motor_pos)) {
		// return false because given point isn't accessible by arm
		return false;
	}

	// move shoulder and upper_elbow (only motors responsible for vertical axis) according to
	// calculated motors positions
	MoveShoulder(shoulder_motor_pos);
	MoveUpperElbow(elbow_motor_pos);

	// moved successfully
	return true;
}

void ArmMover::moveHorizontal(double front, double horizontal) {

	// calculate base rotation angle according to given point (front,horizontal)
	double rotation_angle = (atan(double(front) / double(horizontal + ZERO_DIV_CORRECTION))
							 * 180.0 / PI);

	if (horizontal < 0) {

		rotation_angle -= NEG_ROTATE_CORRECTION;
	}

	// move base_rotation joint according to calculated angle
	RotateBase(getBaseRotationMotorPosition(rotation_angle));
}

void ArmMover::calcAngle(double cord_i, double cord_j, double a, double b,
		double* alpha, double* gamma) {

	// transform_angle is angle that used to rotate the triangle so that
	// the fingers will reach the gripping point
	double transform_angle = (atan(double(cord_j) / double(cord_i + ZERO_DIV_CORRECTION)) * 180.0 / PI);

	// calculate third side of triangle according to coordinates (only two sides are given (a;b))
	double c = getDistance(cord_i, cord_j);

	// now we have three sides of triangle, calculate its angles (only two angles are needed
	// for motors
	*alpha = acos((b * b + c * c - a * a) / (2 * b * c)) * 180.0 / PI;
	*gamma = 180 - (*alpha)
			- acos((a * a + c * c - b * b) / (2 * a * c)) * 180.0 / PI;

	// gamma must use alpha before transformation
	*alpha += transform_angle;
}

double ArmMover::getDistance(double x, double y) {

	// Pythagorean theorem
	return sqrt(x * x + y * y);
}

double ArmMover::getShoulderMotorPosition(double alpha) {

	return 1.5 - (alpha) / 60;
}

double ArmMover::getBaseRotationMotorPosition(double alpha) {

	if (alpha < 0) {

		return 1.5 - (180 + alpha) / 60;
	}

	return 1.5 - (alpha) / 60;
}

double ArmMover::getElbowMotorPosition(double gamma) {

	return 1.5 + (90 - gamma) / 60;
}
